# !/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2024/1/30 9:13
# @Author  : xinyingjie
# @File    : main-dev.py
# @Description :  隧道多断面仿真拼接


from __future__ import absolute_import
from __future__ import print_function

import json
import os
import threading
import uuid
import random
import time
import asyncio
import traci.constants as tc
import sumolib
import traci
from kafka import KafkaProducer

from util.websocketserver1 import WebSocketServer
import kafka_util
from util import sumo_utils
from kafka_util import pre_data_queue, all_data_queue, ai_data_queue
import logging
from util.tools import calculate_distance
from config.config import GLOBAL_POLYGON_CENTER, MAX_SPEED, MIN_SPEED, CAR_DEFAULT_SPEED, TRUCK_DEFAULT_SPEED, \
    SPECIAL_DEFAULT_SPEED, erId_dict, type_change, route_list, KAFKA_HOST,SUMO_TOPIC

car_cache_info = {}
moveTo_info = {}
routeIdList = []
global_vehicles = {}
exists_list = []
slow_car_list = []


# "G152300000000000001": {"lon": 121.363835720355738, "lat": 28.2150032304759},
def get_logger(log_type):
    # 第一步，创建一个logger
    logger = logging.getLogger()
    logger.setLevel(logging.INFO)  # Log等级总开关
    # 第二步，创建一个handler，用于写入日志文件
    rq = str(time.strftime('%Y%m%d', time.localtime(time.time()))) + '_' + str(log_type)
    log_path = 'logs/'
    # log_path = '/opt/python' + '/logs/'
    # print('log_path', log_path)
    if not os.path.exists(log_path):
        os.mkdir(log_path)

    log_name = log_path + rq + '.log'
    logfile = log_name
    fh = logging.FileHandler(logfile)
    fh.setLevel(logging.DEBUG)  # 输出到file的log等级的开关
    # 第三步，定义handler的输出格式
    formatter = logging.Formatter("%(asctime)s - %(filename)s[line:%(lineno)d] - %(levelname)s: %(message)s")
    fh.setFormatter(formatter)
    # 第四步，将logger添加到handler里面
    logger.addHandler(fh)
    return logger


def main(sumo_cfg_file=None, sumo_net_file=None, gui=True):
    global global_vehicles
    sumo_net_instance = sumolib.net.readNet(sumo_net_file, withInternal=True, withLatestPrograms=True)
    sumo_utils.start_sumo(sumo_cfg_file, False, gui=False)
    # 设置所有车道的最大通行速度
    for id in traci.lane.getIDList():
        traci.lane.setMaxSpeed(id, MAX_SPEED)
    traci.polygon.add('GLOBAL_POLYGON_CENTER', GLOBAL_POLYGON_CENTER, (1, 0, 1))
    traci.polygon.subscribeContext('GLOBAL_POLYGON_CENTER',
                                   tc.CMD_GET_VEHICLE_VARIABLE, 10000,
                                   [tc.VAR_SPEED, tc.VAR_DECEL, tc.VAR_DISTANCE, tc.VAR_TYPE, tc.VAR_POSITION,
                                    tc.VAR_ROAD_ID, tc.VAR_LANE_INDEX, tc.VAR_LANE_ID, tc.VAR_ACCEL, tc.VAR_ANGLE,
                                    tc.VAR_LANE_ID, tc.VAR_ACCELERATION, tc.VAR_LANE_INDEX,
                                    tc.TRAFFICLIGHT_TYPE_STATIC])
    traci.simulation.subscribe([tc.VAR_ARRIVED_VEHICLES_NUMBER,
                                tc.VAR_ARRIVED_VEHICLES_IDS,
                                tc.VAR_DEPARTED_VEHICLES_NUMBER,
                                tc.VAR_DEPARTED_VEHICLES_IDS,
                                tc.VAR_COLLIDING_VEHICLES_NUMBER,
                                tc.VAR_COLLIDING_VEHICLES_IDS])
    # 获取虚拟设备状态kafka数据
    threading.Thread(target=kafka_util.receive_message_of_pre_data).start()
    threading.Thread(target=kafka_util.receive_message_of_all_data).start()
    threading.Thread(target=kafka_util.receive_message_of_ai_data).start()
    # ws_server = WebSocketServer()
    # ws_server.start()

    producer = KafkaProducer(bootstrap_servers=KAFKA_HOST, key_serializer=str.encode,
                             value_serializer=lambda x: json.dumps(x).encode('utf-8'))

    EdgeID10, Pos10, LaneIndex10 = traci.simulation.convertRoad(121.15935958921908,28.235049070735247, True)


    for k, v in erId_dict.items():
        x, y = traci.simulation.convertGeo(v["lon"], v["lat"], fromGeo=True)
        EdgeIDk, Posk, LaneIndexk = traci.simulation.convertRoad(v["lon"], v["lat"], True)
        print("EdgeIDk", EdgeIDk)
        erId_dict[k]["x"] = x
        erId_dict[k]["y"] = y
        erId_dict[k]["edge"] = EdgeIDk
        erId_dict[k]["pos"] = Posk
    logger = get_logger("simulation")
    startSimuTime = int(time.time() * 1000)
    try:
    # loop = asyncio.get_event_loop()
        while (True):

            try:
                start_time = time.time()

                #  相机感知结果  增加仿真车
                while not ai_data_queue.empty():
                    vInfo = ai_data_queue.get(block=False)
                    # print(vInfo)
                    PerceptualInfo = vInfo["PerceptualInfo"]
                    for perceptualInfo in PerceptualInfo:
                        carid = str(perceptualInfo["id"])

                        carid = str(carid.encode("utf-8"))

                        if carid not in exists_list:
                            EdgeID, Pos, LaneIndex = traci.simulation.convertRoad(perceptualInfo["lon"], perceptualInfo["lat"],
                                                                                  True)
                            if str(EdgeID) in routeIdList:
                                routeID = str(EdgeID)
                            else:
                                traci.route.add(str(EdgeID), route_list[route_list.index(str(EdgeID)):])
                                routeIdList.append(str(EdgeID))
                                routeID = str(EdgeID)
                            traci.vehicle.add(
                                vehID=carid,
                                routeID=routeID,
                                # typeID=str(vInfo["vehicleType"]), depart='0', departPos=Pos,
                                typeID=str(type_change[int(perceptualInfo["type"])]), depart='0', departPos=Pos,
                                departLane=str(int(perceptualInfo["laneNum"]) - 1), departSpeed=str(CAR_DEFAULT_SPEED))
                            traci.vehicle.setSpeed(carid, CAR_DEFAULT_SPEED)
                            exists_list.append(carid)

                # pre-data更新车牌
                while not pre_data_queue.empty():

                    vInfo = pre_data_queue.get(block=False)
                    car_id = str(vInfo["carId"].encode("utf-8"))
                    print("predata", car_id, list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())))
                    if car_id in exists_list:
                        traci.vehicle.setParameter(car_id, "licenseCode",
                                                   str(vInfo["licenseCode"].encode("utf-8")))
                    else:
                        print("predata匹配失败，增加新车",car_id)
                        try:
                            traci.vehicle.add(
                                vehID=car_id,
                                routeID="route_main",
                                # typeID=str(vInfo["vehicleType"]), depart='0', departPos=Pos,
                                typeID=str(1), depart='0', departPos=Pos10,
                                departLane=str(int(vInfo["headLaneNum"]) - 1), departSpeed=str(CAR_DEFAULT_SPEED))
                            traci.vehicle.setSpeed(car_id, CAR_DEFAULT_SPEED)
                            exists_list.append(car_id)
                            traci.vehicle.setParameter(car_id, "licenseCode",
                                                       str(vInfo["licenseCode"].encode("utf-8")))
                        except Exception as e:
                            print(str(e))
                global_vehicles = traci.polygon.getContextSubscriptionResults('GLOBAL_POLYGON_CENTER')
                delete_list = []
                for k, v in moveTo_info.items():
                    if v["edge"] in routeIdList:
                        routeID = v["edge"]
                    else:
                        traci.route.add(v["edge"], route_list[route_list.index(v["edge"]):])
                        routeIdList.append(v["edge"])
                        routeID = v["edge"]
                    traci.vehicle.add(
                        vehID=k,
                        routeID=routeID,
                        typeID=str(1), depart='0', departPos=v["pos"],
                        departLane=str(random.randint(0, 2)), departSpeed=str(CAR_DEFAULT_SPEED))
                    traci.vehicle.setSpeed(k, CAR_DEFAULT_SPEED)
                    traci.vehicle.setParameter(k, "licenseCode", v["param"])
                    delete_list.append(k)
                for d in delete_list:
                    del moveTo_info[d]
                while not all_data_queue.empty():
                    all_data_info = all_data_queue.get(block=False)
                    carId = all_data_info["carId"]

                    erId = all_data_info["erId"]
                    # print(list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())))
                    # print(etcLicense, etcLicense in list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())),
                    #       erId in erId_dict.keys())
                    # print(list(map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())), erId)
                    # print(all_data_info)
                    if carId in list(
                            map(lambda key: eval(key).decode('utf-8'), global_vehicles.keys())) and erId in erId_dict.keys():
                        erId_lon, erId_lat = erId_dict[erId]["lon"], erId_dict[erId]["lat"]
                        carId = str(carId.encode("utf-8"))
                        pos = global_vehicles[carId][tc.VAR_POSITION]
                        car_lon, car_lat = sumo_utils.xyConversionlatAndLon(sumo_net_instance, pos)
                        dis = calculate_distance(car_lon, car_lat, erId_lon, erId_lat)
                        # 若门架上报信息时候，仿真车在门架后面，仿真车需要加速，否则，需要减速
                        print(carId, "门架匹配成功")
                        if dis <= 300:

                            car_cache_info[carId] = {"erId_lon": erId_lon, "erId_lat": erId_lat}
                            if erId_lon < car_lon:
                                print(carId, "仿真车在门架后面，仿真车需要加速")
                                traci.vehicle.slowDown(carId, MAX_SPEED, 1.0)
                            else:
                                print(carId, "仿真车在门架前面，仿真车需要减速速")
                                traci.vehicle.slowDown(carId, MIN_SPEED, 1.0)
                                slow_car_list.append(carId)

                        else:
                            print("moveToXY")
                            moveTo_info[carId] = {"edge": str(erId_dict[erId]["edge"]),
                                                  "pos": str(erId_dict[erId]["edge"]),
                                                  "param": traci.vehicle.getParameter(carId, "licenseCode")}
                            traci.vehicle.remove(carId)

                car_list = list()
                for vehicle_id, veh_info in dict(global_vehicles).items():
                    pos = veh_info[tc.VAR_POSITION]
                    longitude, latitude = sumo_utils.xyConversionlatAndLon(sumo_net_instance, pos)
                    licenseCode = ""
                    try:
                        licenseCode = traci.vehicle.getParameter(vehicle_id, "licenseCode")
                    except Exception as e:
                        print("车牌获取异常", vehicle_id)
                    # print(global_vehicles.keys())
                    if licenseCode == "" and longitude < 121.15419231355192:
                        print("删除车辆", vehicle_id)
                        try:
                            traci.vehicle.remove(vehicle_id)
                            print("删除车辆成功")
                        except Exception as e:
                            print(str(e))
                    # 当仿真车已经到达门架
                    if vehicle_id in car_cache_info.keys():
                        erId_lon = car_cache_info[vehicle_id]["erId_lon"]
                        erId_lat = car_cache_info[vehicle_id]["erId_lat"]
                        if vehicle_id not in slow_car_list:
                            if longitude <= erId_lon:
                                print(vehicle_id, "车辆经过门架，恢复初始速度")
                                traci.vehicle.slowDown(vehicle_id, CAR_DEFAULT_SPEED, 4.0)
                                traci.vehicle.setSpeed(vehicle_id, CAR_DEFAULT_SPEED)
                                del car_cache_info[vehicle_id]
                        else:
                            dis = calculate_distance(erId_lon,erId_lat,longitude,latitude)
                            if dis <= 10:
                                traci.vehicle.slowDown(vehicle_id, CAR_DEFAULT_SPEED, 4.0)
                                traci.vehicle.setSpeed(vehicle_id, CAR_DEFAULT_SPEED)
                                del car_cache_info[vehicle_id]
                                slow_car_list.remove(vehicle_id)


                    # pos = veh_info[tc.VAR_POSITION]
                    # longitude, latitude = sumo_utils.xyConversionlatAndLon(sumo_net_instance, pos)
                    speed = veh_info[tc.VAR_SPEED]
                    # print(speed)
                    vehicle_type = veh_info[tc.VAR_TYPE]
                    vehicle_id = eval(vehicle_id).decode('utf-8')
                    car = {"id": vehicle_id, "typeCode": vehicle_type, "longitude": longitude,
                           "latitude": latitude, "speed": speed,
                           "angle": veh_info[tc.VAR_ANGLE],
                           "licenseCode": licenseCode if "" != licenseCode else "默A0000",
                           "eventType": "", "cl": ""}

                    car_list.append(car)
                timestamp = int(time.time() * 1000)
                result = {"objs": car_list, "participantNum": len(car_list),
                          "timestamp": timestamp, "startSimuTime": startSimuTime,
                          "globalTimeStamp": timestamp, "simuTime": timestamp - startSimuTime,
                          "count": len(car_list)}
                # print(result)
                producer.send(SUMO_TOPIC, value=result, key="key")
                # loop.run_until_complete(ws_server.broadcast(str(result).replace("'", '"')))
                traci.simulationStep()
                end_time = time.time()

                if start_time and (end_time - start_time) < 0.1:
                    # print("耗时",(end_time - start_time))
                    time.sleep(0.1 - (end_time - start_time))
                else:
                    if start_time:
                        print("单帧耗时大于0.1s", end_time - start_time)
            except Exception as e:
                traci.simulationStep()
                time.sleep(0.05)
                print(str(e))
                continue

    except Exception as e:
        print(str(e))


if __name__ == "__main__":
    sumo_net_file = r".\config\LQW.net.xml"  # 路网文件
    sumo_cfg_file = r".\config\LQW.sumocfg"  # 路网文件
    logger = get_logger("simulation")
    logger.info("test")

    main(sumo_cfg_file=sumo_cfg_file, sumo_net_file=sumo_net_file)
